home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / linalg / svdstep.c < prev    next >
Encoding:
C/C++ Source or Header  |  2001-11-01  |  7.6 KB  |  383 lines
  1. inline static void
  2. chop_small_elements (gsl_vector * d, gsl_vector * f)
  3. {
  4.   const size_t N = d->size;
  5.   double d_i = gsl_vector_get (d, 0);
  6.  
  7.   size_t i;
  8.  
  9.   for (i = 0; i < N - 1; i++)
  10.     {
  11.       double f_i = gsl_vector_get (f, i);
  12.       double d_ip1 = gsl_vector_get (d, i + 1);
  13.  
  14.       if (fabs (f_i) < GSL_DBL_EPSILON * (fabs (d_i) + fabs (d_ip1)))
  15.     {
  16.       gsl_vector_set (f, i, 0.0);
  17.     }
  18.       d_i = d_ip1;
  19.     }
  20.  
  21. }
  22.  
  23. #ifdef JUNK
  24. inline static void
  25. chase_out_zero (gsl_vector * d, gsl_vector * f, double gc[], double gs[])
  26. {
  27.   const size_t n = d->size;
  28.  
  29.   double c = 0.0, s = 1.0;
  30.  
  31.   size_t i;
  32.  
  33.   for (i = 0; i < N - 1; i++)
  34.     {
  35.       double d_i = gsl_vector_get (d, i);
  36.       double f_i = gsl_vector_get (f, i);
  37.  
  38.       double f1 = s * f_i;
  39.  
  40.       gsl_vector_set (f, i, c * f_i);
  41.  
  42.       create_givens (d_i, f1, &c, &s);
  43.  
  44.       gsl_vector_set (d, i, hypot (d_i, f1));
  45.  
  46.       if (gc)
  47.     gc[i] = c;
  48.       if (gs)
  49.     gs[i] = s;
  50.     }
  51. }
  52. #endif
  53.  
  54. static double
  55. trailing_eigenvalue (const gsl_vector * d, const gsl_vector * f)
  56. {
  57.   const size_t n = d->size;
  58.  
  59.   double da = gsl_vector_get (d, n - 2);
  60.   double db = gsl_vector_get (d, n - 1);
  61.   double fa = (n > 2) ? gsl_vector_get (f, n - 3) : 0.0;
  62.   double fb = gsl_vector_get (f, n - 2);
  63.  
  64.   double ta = da * da + fa * fa;
  65.   double tb = db * db + fb * fb;
  66.   double tab = da * fb;
  67.  
  68.   double dt = (ta - tb) / 2.0;
  69.  
  70.   double mu;
  71.  
  72.   if (dt >= 0)
  73.     {
  74.       mu = tb - (tab * tab) / (dt + hypot (dt, tab));
  75.     }
  76.   else
  77.     {
  78.       mu = tb + (tab * tab) / ((-dt) + hypot (dt, tab));
  79.     }
  80.  
  81.   return mu;
  82. }
  83.  
  84. static void
  85. create_schur (double d0, double f0, double d1, double * c, double * s)
  86. {
  87.   double apq = 2.0 * d0 * f0;
  88.   
  89.   if (apq != 0.0)
  90.     {
  91.       double t;
  92.       double tau = (f0*f0 + (d1 + d0)*(d1 - d0)) / apq;
  93.       
  94.       if (tau >= 0.0)
  95.         {
  96.           t = 1.0/(tau + hypot(1.0, tau));
  97.         }
  98.       else
  99.         {
  100.           t = -1.0/(-tau + hypot(1.0, tau));
  101.         }
  102.  
  103.       *c = 1.0 / hypot(1.0, t);
  104.       *s = t * (*c);
  105.     }
  106.   else
  107.     {
  108.       *c = 1.0;
  109.       *s = 0.0;
  110.     }
  111. }
  112.  
  113. static void
  114. svd2 (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
  115. {
  116.   size_t i;
  117.   double c, s, a11, a12, a21, a22;
  118.  
  119.   const size_t M = U->size1;
  120.   const size_t N = V->size1;
  121.  
  122.   double d0 = gsl_vector_get (d, 0);
  123.   double f0 = gsl_vector_get (f, 0);
  124.   
  125.   double d1 = gsl_vector_get (d, 1);
  126.  
  127.   if (d0 == 0.0)
  128.     {
  129.       /* Eliminate off-diagonal element in [0,f1;0,d1] to make [d,0;0,0] */
  130.  
  131.       create_givens (f0, d1, &c, &s);
  132.  
  133.       /* compute B <= G^T B X,  where X = [0,1;1,0] */
  134.  
  135.       gsl_vector_set (d, 0, c * f0 - s * d1);
  136.       gsl_vector_set (f, 0, s * f0 + c * d1);
  137.       gsl_vector_set (d, 1, 0.0);
  138.       
  139.       /* Compute U <= U G */
  140.  
  141.       for (i = 0; i < M; i++)
  142.     {
  143.       double Uip = gsl_matrix_get (U, i, 0);
  144.       double Uiq = gsl_matrix_get (U, i, 1);
  145.       gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
  146.       gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
  147.     }
  148.  
  149.       /* Compute V <= V X */
  150.  
  151.       gsl_matrix_swap_columns (V, 0, 1);
  152.  
  153.       return;
  154.     }
  155.   else if (d1 == 0.0)
  156.     {
  157.       /* Eliminate off-diagonal element in [d0,f1;0,0] */
  158.  
  159.       create_givens (d0, f0, &c, &s);
  160.  
  161.       /* compute B <= B G */
  162.  
  163.       gsl_vector_set (d, 0, d0 * c - f0 * s);
  164.       gsl_vector_set (f, 0, 0.0);
  165.  
  166.       /* Compute V <= V G */
  167.  
  168.       for (i = 0; i < N; i++)
  169.         {
  170.           double Vip = gsl_matrix_get (V, i, 0);
  171.           double Viq = gsl_matrix_get (V, i, 1);
  172.           gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
  173.           gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
  174.         }
  175.  
  176.       return;
  177.     }
  178.   else
  179.     {
  180.       /* Make columns orthogonal, A = [d0, f1; 0, d1] * G */
  181.       
  182.       create_schur (d0, f0, d1, &c, &s);
  183.       
  184.       /* compute B <= B G */
  185.       
  186.       a11 = c * d0 - s * f0;
  187.       a21 = - s * d1;
  188.       
  189.       a12 = s * d0 + c * f0;
  190.       a22 = c * d1;
  191.       
  192.       /* Compute V <= V G */
  193.       
  194.       for (i = 0; i < N; i++)
  195.         {
  196.           double Vip = gsl_matrix_get (V, i, 0);
  197.           double Viq = gsl_matrix_get (V, i, 1);
  198.           gsl_matrix_set (V, i, 0, c * Vip - s * Viq);
  199.           gsl_matrix_set (V, i, 1, s * Vip + c * Viq);
  200.         }
  201.       
  202.       /* Eliminate off-diagonal elements, bring column with largest
  203.          norm to first column */
  204.       
  205.       if (hypot(a11, a21) < hypot(a12,a22))
  206.         {
  207.           double t1, t2;
  208.  
  209.           /* B <= B X */
  210.  
  211.           t1 = a11; a11 = a12; a12 = t1;
  212.           t2 = a21; a21 = a22; a22 = t2;
  213.  
  214.           /* V <= V X */
  215.  
  216.           gsl_matrix_swap_columns(V, 0, 1);
  217.         } 
  218.  
  219.       create_givens (a11, a21, &c, &s);
  220.       
  221.       /* compute B <= G^T B */
  222.       
  223.       gsl_vector_set (d, 0, c * a11 - s * a21);
  224.       gsl_vector_set (f, 0, c * a12 - s * a22);
  225.       gsl_vector_set (d, 1, s * a12 + c * a22);
  226.       
  227.       /* Compute U <= U G */
  228.       
  229.       for (i = 0; i < M; i++)
  230.         {
  231.           double Uip = gsl_matrix_get (U, i, 0);
  232.           double Uiq = gsl_matrix_get (U, i, 1);
  233.           gsl_matrix_set (U, i, 0, c * Uip - s * Uiq);
  234.           gsl_matrix_set (U, i, 1, s * Uip + c * Uiq);
  235.         }
  236.  
  237.       return;
  238.     }
  239. }
  240.  
  241.  
  242.  
  243. static void
  244. qrstep (gsl_vector * d, gsl_vector * f, gsl_matrix * U, gsl_matrix * V)
  245. {
  246.   const size_t M = U->size1;
  247.   const size_t N = V->size1;
  248.   const size_t n = d->size;
  249.   double y, z;
  250.   double ak, bk, zk, ap, bp, aq, bq;
  251.   size_t i, k;
  252.  
  253.   if (n == 2)
  254.     {
  255.       svd2 (d, f, U, V);
  256.       return;
  257.     }
  258.  
  259.   {
  260.     double d0 = gsl_vector_get (d, 0);
  261.     double f0 = gsl_vector_get (f, 0);
  262.     
  263.     double d1 = gsl_vector_get (d, 1);
  264.     double f1 = (n > 2) ? gsl_vector_get (f, 1) : 0.0;
  265.     
  266.     {
  267.       double mu = trailing_eigenvalue (d, f);
  268.     
  269.       y = d0 * d0 - mu;
  270.       z = d0 * f0;
  271.     }
  272.     
  273.     /* Set up the recurrence for Givens rotations on a bidiagonal matrix */
  274.     
  275.     ak = 0;
  276.     bk = 0;
  277.     
  278.     ap = d0;
  279.     bp = f0;
  280.     
  281.     aq = d1;
  282.     bq = f1;
  283.   }
  284.  
  285.   for (k = 0; k < n - 1; k++)
  286.     {
  287.       double c, s;
  288.       create_givens (y, z, &c, &s);
  289.  
  290.       /* Compute V <= V G */
  291.  
  292.       for (i = 0; i < N; i++)
  293.     {
  294.       double Vip = gsl_matrix_get (V, i, k);
  295.       double Viq = gsl_matrix_get (V, i, k + 1);
  296.       gsl_matrix_set (V, i, k, c * Vip - s * Viq);
  297.       gsl_matrix_set (V, i, k + 1, s * Vip + c * Viq);
  298.     }
  299.  
  300.       /* compute B <= B G */
  301.  
  302.       {
  303.     double bk1 = c * bk - s * z;
  304.  
  305.     double ap1 = c * ap - s * bp;
  306.     double bp1 = s * ap + c * bp;
  307.     double zp1 = -s * aq;
  308.  
  309.     double aq1 = c * aq;
  310.  
  311.     if (k > 0)
  312.       {
  313.         gsl_vector_set (f, k - 1, bk1);
  314.       }
  315.  
  316.     ak = ap1;
  317.     bk = bp1;
  318.     zk = zp1;
  319.  
  320.     ap = aq1;
  321.  
  322.     if (k < n - 2)
  323.       {
  324.         bp = gsl_vector_get (f, k + 1);
  325.       }
  326.     else
  327.       {
  328.         bp = 0.0;
  329.       }
  330.  
  331.     y = ak;
  332.     z = zk;
  333.       }
  334.  
  335.       create_givens (y, z, &c, &s);
  336.  
  337.       /* Compute U <= U G */
  338.  
  339.       for (i = 0; i < M; i++)
  340.     {
  341.       double Uip = gsl_matrix_get (U, i, k);
  342.       double Uiq = gsl_matrix_get (U, i, k + 1);
  343.       gsl_matrix_set (U, i, k, c * Uip - s * Uiq);
  344.       gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
  345.     }
  346.  
  347.       /* compute B <= G^T B */
  348.  
  349.       {
  350.     double ak1 = c * ak - s * zk;
  351.     double bk1 = c * bk - s * ap;
  352.     double zk1 = -s * bp;
  353.  
  354.     double ap1 = s * bk + c * ap;
  355.     double bp1 = c * bp;
  356.  
  357.     gsl_vector_set (d, k, ak1);
  358.  
  359.     ak = ak1;
  360.     bk = bk1;
  361.     zk = zk1;
  362.  
  363.     ap = ap1;
  364.     bp = bp1;
  365.  
  366.     if (k < n - 2)
  367.       {
  368.         aq = gsl_vector_get (d, k + 2);
  369.       }
  370.     else
  371.       {
  372.         aq = 0.0;
  373.       }
  374.  
  375.     y = bk;
  376.     z = zk;
  377.       }
  378.     }
  379.  
  380.   gsl_vector_set (f, n - 2, bk);
  381.   gsl_vector_set (d, n - 1, ap);
  382. }
  383.